/*******************************************************************************
 *     Description: MAXQ622 Autonomous Car - SRF02 I2C Distance Sensor
 *             $Id$
 *******************************************************************************
 */

#include "maxq_config.h"
#include "i2c/maxq_i2c.h"
#include "comm/maxq_comm.h"
#include "dist.h"

/***** Definitions *****/
#define SPEED_OF_SOUND  340290

// Distance sensor register offsets
#define REG_SW_REV      0
#define REG_CMD         0
#define REG_DIST_LOW    2
#define REG_DIST_HIGH   1

// Distance sensor commands
#define CMD_RANGE_IN    0x50
#define CMD_RANGE_CM    0x51
#define CMD_RANGE_US    0x52

#define MAX_RANGE_CM    600

/***** Global Data *****/
int distance[NUM_SENSORS];


/***** File Scope Data *****/
static int curr_sensor;


/****************************************************************************/
int dist_init(void)
{
  int i;
  uint8_t msg;

  // initialize 
  i2c_master_init(I2C_STD_MODE);
//  i2c_enable(); // this must be skipped if SW I2C is used

  // Initialize the current sensor pointer
  curr_sensor = 0;

  for (i = 0; i < NUM_SENSORS; i++) {
    distance[i] = -1;
    // read the SW version
    if (!i2c_master_read(SENSOR_ADDR(i), 1, &msg))
      return 0;
  }

  return 1;
}

/****************************************************************************/
int dist_task(void)
{
  uint8_t msg[5];
  unsigned int temp16;
  int retval = 0;

  // Read the previous distance result
  if (!i2c_master_read(SENSOR_ADDR(curr_sensor), 5, msg)) {
    distance[curr_sensor] = -1;
    retval = -1;
  }
  else {
    // The sensor will not respond until the measurement is complete
    if (msg[REG_SW_REV] == 0xFF) {
      retval = 0;
    }
    else if (msg[REG_SW_REV] != 0x0A) {
      distance[curr_sensor] = -1;
      retval = -1;
maxq_printf("Dist %u Version Err: msg %02x %02x %02x %02x %02x\r\n", curr_sensor, msg[0], msg[1], msg[2], msg[3], msg[4]);
    }
    else {
      // Update the sensor distance
      temp16 = msg[REG_DIST_HIGH];
      temp16 <<= 8;
      temp16 += msg[REG_DIST_LOW];
      temp16 = (temp16 + 1) >> 1;

      if (temp16 == 0) {
        retval = 0;
maxq_printf("Dist %u Zero: msg %02x %02x %02x %02x %02x\r\n", curr_sensor, msg[0], msg[1], msg[2], msg[3], msg[4]);
      } else if (temp16 <= MAX_RANGE_CM) {
        distance[curr_sensor] = temp16;
        retval = 1;
      } else {
        distance[curr_sensor] = -1;
        retval = -1;
maxq_printf("Dist %u Max Range Err: %4d, msg %02x %02x %02x %02x %02x\r\n", curr_sensor, temp16, msg[0], msg[1], msg[2], msg[3], msg[4]);
      }
    }
  }

//maxq_printf("Distance: %4d cm, msg %02x %02x %02x %02x\r\n", distance[0], msg[0], msg[1], msg[2], msg[3]);

  // Update the current sensor pointer
  curr_sensor++;
  if (curr_sensor >= NUM_SENSORS)
    curr_sensor = 0;

  // Initiate the next distance measurement
  msg[0] = 0;     // command register is at address 0
  msg[1] = CMD_RANGE_CM;  // initiate measurement
  i2c_master_write(SENSOR_ADDR(curr_sensor), 2, msg);

  return retval;
}

/****************************************************************************/
int dist_set_id(int curr_id, int new_id)
{
  uint8_t msg[2];

  if (curr_id < 0)
    curr_id = SENSOR_ADDR_BASE;

  msg[0] = 0;     // command register is at address 0

  msg[1] = 0xA0;
  if (!i2c_master_write(curr_id, 2, msg))
    return 0;

  msg[1] = 0xAA;
  if (!i2c_master_write(curr_id, 2, msg))
    return 0;

  msg[1] = 0xA5;
  if (!i2c_master_write(curr_id, 2, msg))
    return 0;

  msg[1] = new_id << 1;
  if (!i2c_master_write(curr_id, 2, msg))
    return 0;

  return 1;
}
